package robins;

import geometry.AnalyticGeometry;
import network.Constants;
import network.Gateway;

import java.util.ArrayList;

/**
 * @author: Fatih Senel
 * Date: Jun 9, 2010
 * Time: 1:44:53 PM
 */
public class Robins {

    final int INT_MAX = 32767;
    final int IGNORE = -100;


    int N_SP;
    int[] combSeg;
    int[] combSP;
    SteinerT[] H;
    SteinerT[] T;
    SteinerT[] K;
    SteinerT[] MST_T_K;
    SteinerT[] best_K = new SteinerT[Constants.MAX_EL_ST];
    LOSS_K[] best_Loss_K = new LOSS_K[Constants.MAX_EL_ST];

    static int N_edges_H = 0;
    static int N_nodes_H = 0;
    private int cost_T;
    NODE[] node;
    NODE[] All_SegNode;
    private int N_nodes_T;
    private int N_nodes_K;
    private int N_edges_T;
    private int N_edges_K;
    LOSS_K[] Loss_K;

    int Num_best_r, Num_best_K, cntSeg, best_N_nodes_K,
            best_N_edges_K, cost_K, cost_Loss_K, cost_MST_T_K,
            N_edges_MST_T_K, N_Forest_Loss_K, best_N_Forest_Loss_K,
            N_RN_Robins, total_node_dgr_Robins;

    int get_bug = 0;

    double best_r, r;
    ArrayList<SEGMENT> segm;
    int N_seg;
    int k_restricted;
    int col_N_sq, row_N_sq;
    int initSP, SPrange;
    NODE[] k_SegNode;
    int[] triedSP;

    public Robins(ArrayList<Gateway> actorsArray) {
        segm = new ArrayList<SEGMENT>();
        for (int i = 0; i < actorsArray.size(); i++) {
            Gateway gateway = actorsArray.get(i);
            SEGMENT sg = new SEGMENT();
            sg.id = gateway.getID();
            sg.loc_r = (int) gateway.getY();
            sg.loc_c = (int) gateway.getX();
            segm.add(sg);
        }
        N_seg = segm.size();
        k_restricted = 3;
        k_SegNode = new NODE[Constants.MAX_N_SEGM];
        // todo temporarily assigned
        col_N_sq = 14;
        row_N_sq = 14;

        processAPathByRobins();
    }

    public void processAPathByRobins() {
        int i, cost, N_node, N_edges_out;
        SteinerT[] out = new SteinerT[Constants.MAX_EL_ST];
        All_SegNode = new NODE[N_seg];

        for (i = 0; i < N_seg; i++) {
            NODE tmp = new NODE();
            tmp.id = segm.get(i).id;
            tmp.c = segm.get(i).loc_c;
            tmp.r = segm.get(i).loc_r;
            All_SegNode[i] = tmp;
        }

        N_SP = Constants.k_restricted - 2;
        combSP = new int[Constants.MAX_N_SEGM];
        combSeg = new int[Constants.MAX_N_SEGM];

        Init_H();
        Init_T();
        Num_best_r = 0;
        r = 1;
        Num_best_K = 0;


        while (r > 0) {

            /* Try all combinations of (all combination of Seg and SPs)
         * Find the best r */
            best_r = -100.00;
            Robins_CombinationRe(k_restricted);

            r = best_r;
            if (r <= 0) {
                break;
            }

            Num_best_r++;
//            print_best();
            Update_H();
            Update_T();
            Num_best_K++;

        } /* End of while */

//    printf("Algorithm terminates and finds MST(H)\n");

        if (Num_best_r == 0) {

            /* add N_seg on 041309 */
            N_RN_Robins = Count_RN_on_out(T, N_edges_T) + N_seg;

//	printf("Best_r has been NEVER found!!\n");
//    	printf("N_RN_Robins = %d\n",N_RN_Robins);

            total_node_dgr_Robins = Num_best_K + Count_Total_Node_Dgr_MST_RNP(N_RN_Robins);
            return;
        }

        /* Remove duplicated Node IDs used for SPs */
        Remove_duplicatedNodeID(H, N_edges_H);

        /* Initialize a temp. global var. node */
        N_nodes_H = 0;
        node = new NODE[Constants.MAX_EL_ST];
        N_nodes_H = Count_nodes_of_graph(H, N_edges_H, node);

        N_edges_out = 0;
        out = new SteinerT[Constants.MAX_EL_ST];
        Init_touched_H();
        int[] res = Get_MST(H, N_nodes_H, N_edges_H, out);
        cost = res[0];
        N_edges_out = res[1];

        /* add N_seg on 041309 */
        N_RN_Robins = N_SP + Count_RN_on_out(out, N_edges_out) + N_seg;

/* PRINT */
/*#if RESULT_PRINT || DEBUG_ROBINS || PRINT_FINAL_MST
    printf("Final MST: ");
    for ( i = 0; i < N_edges_out; i++ )
	printf("{%d(%d,%d)-%d(%d,%d),%d,%d}, ", out[i].u_id, out[i].u_r, out[i].u_c, out[i].v_id, out[i].v_r, out[i].v_c, out[i].cellDist, out[i].touched);
    printf("\n");
    printf("N_RN_Robins = %d\n",N_RN_Robins);

	//fatih begin
	for ( i = 0; i < N_seg; i++ ) {
		printf("Segment[%d]=(%d, %d)\n",segm[i].id, segm[i].loc_c, segm[i].loc_r);
	}
	//fatih end


#endif*/

        total_node_dgr_Robins = Count_Total_Node_Dgr_MST_RNP(N_RN_Robins);
        return;


    }

    private void Robins_CombinationRe(int k) {
        int i, OuterSlotIndex = 0, OuterMaxValue = N_seg - 1;

        for (i = 0; i < N_seg; i++)
            combSeg[i] = i;

        cntSeg = 0;
        OuterFillSlot(k, OuterSlotIndex, OuterMaxValue);

    }

    void OuterFillSlot(int k, int OuterSlotIndex, int OuterMaxValue) {
        int same, i, j, from, to,
                OuterMinValue,
                InnerSlotIndex, InnerMaxValue; /* <= N_sq-1 */

        if (OuterSlotIndex == k) {

            same = 0;
            for (i = 0; i < k; i++)
                for (j = i + 1; j < k; j++)
                    if (combSeg[j] == combSeg[i]) same = 1;

            if (same == 1) return;

            k_SegNode = new NODE[k_restricted];

            for (i = 0; i < k_restricted; i++) {
                NODE tmp = new NODE();
                tmp.id = segm.get(combSeg[i]).id;
                tmp.r = segm.get(combSeg[i]).loc_r;
                tmp.c = segm.get(combSeg[i]).loc_c;
                k_SegNode[i] = tmp;

            }

            InnerMaxValue = -1;
            InnerSlotIndex = InnerMaxValue;
            int[] res = GetBoundary();
            from = res[0];
            to = res[1];
            if (from == -1 || to == -1) {
                System.out.println(" Error in OuterFillSlot \n");
                System.exit(0);
            }

            cntSeg = 0;
            SPrange = to - from - 1;
            initSP = from;
            InnerSlotIndex = 0;
            InnerMaxValue = SPrange - 1;
            InnerFillSlot(N_SP, InnerSlotIndex, InnerMaxValue);

        } else {
            OuterMinValue = 0;
            if (OuterSlotIndex > 0)
                OuterMinValue = combSeg[OuterSlotIndex - 1];

            for (i = OuterMinValue; i <= OuterMaxValue; i++) {
                combSeg[OuterSlotIndex] = i;
                OuterFillSlot(k, OuterSlotIndex + 1, OuterMaxValue);
            }
        }

    }

    void InnerFillSlot(int n_SP, int InnerSlotIndex, int InnerMaxValue) {
        int i, InnerMinValue;
        double a_r;

        if (InnerSlotIndex == n_SP) {
            triedSP = new int[Constants.MAX_N_SEGM];

            for (i = 0; i < N_SP; i++) {
                triedSP[i] = combSP[i] + initSP;
            }

            a_r = Compute_r();
            System.out.println("a_r = " + a_r + "\t best_r = " + best_r);
            if (a_r > best_r) {
                best_r = a_r;
                best_N_nodes_K = N_nodes_K;
                best_N_edges_K = N_edges_K;

                System.arraycopy(K, 0, best_K, 0, N_edges_K);

                best_N_Forest_Loss_K = N_Forest_Loss_K;

                System.arraycopy(Loss_K, 0, best_Loss_K, 0, N_Forest_Loss_K);

            }

        } else {
            InnerMinValue = 0;
            if (InnerSlotIndex > 0)
                InnerMinValue = combSP[InnerSlotIndex - 1];

            for (i = InnerMinValue; i <= InnerMaxValue; i++) {
                combSP[InnerSlotIndex] = i;
                InnerFillSlot(n_SP, InnerSlotIndex + 1, InnerMaxValue);

            }
        }

    }


    /* Get a complete graph over all segments */
    void Init_H() {
        int i, j;
        double len;

//        memset(H, 0x00, sizeof(SteinerT) * MAX_EL_ST);
        H = new SteinerT[Constants.MAX_EL_ST];
        N_edges_H = 0;
        for (i = 0; i < N_seg; i++)
            for (j = i + 1; j < N_seg; j++) {
                SteinerT tmp = new SteinerT();
                tmp.u_id = segm.get(i).id;
                tmp.u_r = segm.get(i).loc_r;
                tmp.u_c = segm.get(i).loc_c;

                tmp.v_id = segm.get(j).id;
                tmp.v_r = segm.get(j).loc_r;
                tmp.v_c = segm.get(j).loc_c;

                H[N_edges_H] = tmp;

                len = AnalyticGeometry.euclideanDistance(H[N_edges_H].u_r, H[N_edges_H].u_c, H[N_edges_H].v_r, H[N_edges_H].v_c);
                H[N_edges_H].cellDist = (int) len;

                H[N_edges_H].touched = 0;
                N_edges_H++;
            }
        N_nodes_H = N_seg;
        N_edges_H = (N_seg * (N_seg - 1)) / 2;
    }

    /**
     * Find a MST over H
     */
    void Init_T() {

        int i;
        node = new NODE[N_seg];
        System.arraycopy(All_SegNode, 0, node, 0, node.length);
        T = new SteinerT[Constants.MAX_EL_ST];
        int[] res = Get_MST(H, N_seg, N_edges_H, T);
        cost_T = res[0];
        N_edges_T = res[1];
        N_nodes_T = N_seg;
    }

    private int[] Get_MST(SteinerT[] in, int N_V, int no_in_edges, SteinerT[] out) {
        int i, j, u, u_r = -1, u_c = -1, v, v_r = -1, v_c = -1,
                min, idx, cost, N_edges, chosenEdge = -1, parent,
                child, newJoin = -1;


        for (i = 0; i < k_restricted + N_SP; i++) {
            print_node(i);
        }
        System.out.println("--------------\n");

        node[0].included = 1;
        N_edges = 0;
        cost = 0;

        while (N_edges < N_V - 1) {
            min = INT_MAX;
            v = -1;
            u = v;
            for (i = 0; i < N_V; i++) {
                if (node[i].included == 1) {
                    parent = node[i].id;
                    for (j = 0; j < no_in_edges; j++) {
                        if (in[j].touched == 0) {
                            if (in[j].u_id == parent) {
                                child = in[j].v_id;
                            } else if ((in[j]).v_id == parent) {
                                child = (in[j]).u_id;
                            } else {
                                continue;
                            }

                            if (IsNodeTouched(N_V, child) == 1) {
                                in[j].touched = 1;
                                continue;
                            }
                            if (in[j].cellDist < min) {
                                u = in[j].u_id;
                                u_r = in[j].u_r;
                                u_c = in[j].u_c;
                                v = in[j].v_id;
                                v_r = in[j].v_r;
                                v_c = in[j].v_c;
                                min = in[j].cellDist;
                                chosenEdge = j;
                                newJoin = child;
                            }
                        } /* if */
                    } /* for */
                } /* if */
            } /* for */

            if (u == -1 || v == -1) {
                System.out.println("Logic error in Get_MST\n");
                //System.exit(0);
                return null;
            }

            CountOutNode(N_V, newJoin);
            out[N_edges] = new SteinerT();
            out[N_edges].u_id = u;
            out[N_edges].u_r = u_r;
            out[N_edges].u_c = u_c;
            out[N_edges].v_id = v;
            out[N_edges].v_r = v_r;
            out[N_edges].v_c = v_c;
            out[N_edges].cellDist = min;
            in[chosenEdge].touched = 1;
            cost += min;
            /* SteinerT tmp = new SteinerT();
          tmp.u_id = u;
          tmp.u_r = u_r;
          tmp.u_c = u_c;
          tmp.v_id = v;
          tmp.v_r = v_r;
          tmp.v_c = v_c;
          tmp.cellDist = min;
          tmp.touched = 1;
          cost += min;
          out[N_edges] = tmp;
          in[chosenEdge].touched = 1;*/


            N_edges++;

        } /* while */

        int[] res = new int[2];
        res[0] = cost;
        res[1] = N_edges;
        return res;

    }

    private int IsNodeTouched(int N_nodes, int NodeId) {
        int i;

        for (i = 0; i < N_nodes; i++) {
            if (node[i].id == NodeId)
                return node[i].included;
        }
        return 0;
    }


    void CountOutNode(int N_nodes, int NodeId) {
        int i;

        for (i = 0; i < N_nodes; i++) {
            if (node[i].id == NodeId && node[i].included == 0) {
                node[i].included = 1;
                return;
            }
        }
        System.out.println(" Error: Node is touched twice during MST (NodeId:" + NodeId);
        System.exit(0);
    }

    void Init_touched_H() {
        int i;

        for (i = 0; i < N_edges_H; i++)
            H[i].touched = 0;
    }

    /*
        int Find_MST_T_K()
        {
            int 	i, N_nodes, cost, N_edges_in;
            SteinerT	in[MAX_EL_ST];

            ///* Get T U K
            N_edges_in = 0;
            N_edges_in = Union_2_graphs( T, N_edges_T, K, N_edges_K, in );

            // Initialize a temp. global var. node
            N_nodes = 0;
            node = new NODE[Constants.MAX_EL_ST];

            N_nodes = Count_nodes_of_graph( in, N_edges_in, node );

            N_edges_MST_T_K = 0;
            memset( MST_T_K, 0x00, sizeof(SteinerT)*MAX_EL_ST);
            return Get_MST( in, N_nodes, N_edges_in, MST_T_K, &N_edges_MST_T_K );
        }
    */

    int[] GetBoundary() {
        int i, min_r, max_r, min_c, max_c;

        min_r = max_r = segm.get(combSeg[0]).loc_r;
        min_c = max_c = segm.get(combSeg[0]).loc_c;
        for (i = 1; i < k_restricted; i++) {

            if (min_r > segm.get(combSeg[i]).loc_r)
                min_r = segm.get(combSeg[i]).loc_r;
            else if (max_r < segm.get(combSeg[i]).loc_r)
                max_r = segm.get(combSeg[i]).loc_r;

            if (min_c > segm.get(combSeg[i]).loc_c)
                min_c = segm.get(combSeg[i]).loc_c;
            else if (max_c < segm.get(combSeg[i]).loc_c)
                max_c = segm.get(combSeg[i]).loc_c;

        }

        /* debugged on 03202009 */
        int[] res = new int[2];
        res[0] = col_N_sq * (min_r) + (min_c + 1);
        res[1] = col_N_sq * (max_r) + (max_c + 1);
        return res;
    }

    double Compute_r() {
        int N_edges, gain;
        double r = 0.0;

        cost_K = Find_K();
        if (cost_K == -1) {
            /* case in which a trying SP is one of segments */
            return -1;
        }

        int[] res = Find_MST_T_K();
        cost_MST_T_K = res[0];
        N_edges_MST_T_K = res[1];

/* gain_T(K) = cost(T) - cost(MST(T U K)) */
        gain = cost_T - cost_MST_T_K;

        cost_Loss_K = Find_Loss_K(N_SP);
        if (cost_Loss_K == IGNORE)
            return -1;

/* Variation for our grid structure */
        if (cost_Loss_K == 0)
            cost_Loss_K = 1;

        r = (double) gain / (double) cost_Loss_K;
        return r;
    }


    int Find_K() {
        int i, N_nodes, N_edges_in, N_edges, cost;
        SteinerT[] in = new SteinerT[Constants.MAX_EL_ST];

        N_nodes = k_restricted + N_SP;
        N_edges = (N_nodes * (N_nodes - 1)) / 2;

        node = new NODE[N_nodes];

        System.arraycopy(k_SegNode, 0, node, 0, k_restricted);

        for (i = 0; i < N_SP; i++) {
            node[i + k_restricted] = new NODE();
            node[i + k_restricted].id = Get_SP_ID(i);
            node[i + k_restricted].r = triedSP[i] / col_N_sq;
            node[i + k_restricted].c = triedSP[i] - (node[i + k_restricted].r * col_N_sq);
        }

        N_edges_in = 0;
        N_edges_in = Create_completeGraph(in, N_nodes);
        if (N_edges_in == -1) {
            /* case in which a trying SP is one of segments */
            return -1;
        }

        N_edges_K = 0;
        K = new SteinerT[N_nodes - 1];
        int[] res = Get_MST(in, N_nodes, N_edges, K);
        cost = res[0];
        N_edges_K = res[1];
        N_nodes_K = N_nodes;

        if (N_edges_K != N_nodes - 1) {
            System.out.println("Logic error in Find_K()\n");
            System.exit(0);
        }

        return cost;
    }

    int[] Find_MST_T_K() {
        int i, N_nodes, cost, N_edges_in;
        SteinerT[] in = new SteinerT[Constants.MAX_EL_ST];

/* Get T U K */
        N_edges_in = 0;
        N_edges_in = Union_2_graphs(T, N_edges_T, K, N_edges_K, in);

        System.out.println("Printing in\n");
        print_SteinerT(in, N_edges_in);
        System.out.println("-----------------\n");

/* Initialize a temp. global var. node */
        N_nodes = 0;
        node = new NODE[Constants.MAX_EL_ST];
        N_nodes = Count_nodes_of_graph(in, N_edges_in, node);

        N_edges_MST_T_K = 0;
        MST_T_K = new SteinerT[Constants.MAX_EL_ST];
        return Get_MST(in, N_nodes, N_edges_in, MST_T_K);
    }

    /* Give a new ID to SP */
    int Get_SP_ID(int i_th) {
        return (N_nodes_H + i_th);
    }

    /* return -1 if trying SP is one of terminal */
    int Create_completeGraph(SteinerT[] in, int N_node) {
        int i, j, idx;
        double len;

        idx = 0;

//        in = new SteinerT[Constants.MAX_EL_ST];

        for (i = 0; i < N_node; i++) {
            for (j = i + 1; j < N_node; j++) {
                in[idx] = new SteinerT();
                in[idx].u_id = node[i].id;
                in[idx].u_r = node[i].r;
                in[idx].u_c = node[i].c;
                in[idx].v_id = node[j].id;
                in[idx].v_r = node[j].r;
                in[idx].v_c = node[j].c;
                if (node[i].r == node[j].r && node[i].c == node[j].c)
                    return -1;

                len = 0.0;
                len = AnalyticGeometry.euclideanDistance(node[i].c, node[i].r, node[j].c, node[j].r);
                in[idx].cellDist = (int) len;

                in[idx].touched = 0;
                idx++;
            }
        }
        return idx;
    }


    int Union_2_graphs(SteinerT[] in1, int N_edges_in1, SteinerT[] in2, int N_edges_in2, SteinerT[] out) {
        int N_edges_out;

        N_edges_out = N_edges_in1 + N_edges_in2;
        if (N_edges_out > Constants.MAX_EL_ST) {
            System.out.println("Limit error in Union_2_graphs\n");
            System.exit(0);
        }
//        out = new SteinerT[Constants.MAX_EL_ST];
        System.arraycopy(in1, 0, out, 0, N_edges_in1);
        System.arraycopy(in2, 0, out, N_edges_in1, N_edges_in2);

//    memcpy( out, in1, sizeof(SteinerT)*N_edges_in1);
//    memcpy( out+N_edges_in1, in2, sizeof(SteinerT)*N_edges_in2);

        return N_edges_out;
    }

    /* Store node ids in G_nodes and return No_nodes in G */
    int Count_nodes_of_graph(SteinerT[] in, int N_edges_in, NODE[] node_) {
        int i, j, N_nodes, same;
//        node_ = new NODE[Constants.MAX_EL_ST];
        NODE tmp1 = new NODE();
        NODE tmp2 = new NODE();
        tmp1.id = in[0].u_id;
        tmp1.r = in[0].u_r;
        tmp1.c = in[0].u_c;
        node_[0] = tmp1;

        tmp2.id = in[0].v_id;
        tmp2.r = in[0].v_r;
        tmp2.c = in[0].v_c;
        node_[1] = tmp2;

        N_nodes = 2;

        for (i = 1; i < N_edges_in; i++) {
            same = 0;
            for (j = 0; j < N_nodes; j++) {
                if (in[i].u_id == node[j].id) {
                    same = 1;
                }
            }
            if (same == 0) {
                node_[N_nodes] = new NODE();
                node_[N_nodes].id = in[i].u_id;
                node_[N_nodes].r = in[i].u_r;
                node_[N_nodes++].c = in[i].u_c;
            }

            same = 0;
            for (j = 0; j < N_nodes; j++) {
                if (in[i].v_id == node[j].id) {
                    same = 1;
                }
            }
            if (same == 0) {
                node_[N_nodes] = new NODE();
                node_[N_nodes].id = in[i].v_id;
                node_[N_nodes].r = in[i].v_r;
                node_[N_nodes++].c = in[i].v_c;
            }
        }
        return N_nodes;
    }


    int Find_Loss_K(int N_sp) {
        int i, path_cost, SP, min, Seg, loss_k, idx,
                tmp_N_edges, TmpBest_N_edges = -1;

        LOSS_K[] tmp = new LOSS_K[Constants.MAX_EL_ST];
        LOSS_K[] TmpBest_Loss_K = new LOSS_K[Constants.MAX_EL_ST];
        Loss_K = new LOSS_K[Constants.MAX_EL_ST];

        N_Forest_Loss_K = 0;
        for (i = 0; i < N_sp; i++) {
            SP = Get_SP_ID(i);

/* Get the shortest path from each SP to each of segments */
            idx = 0;
            min = INT_MAX;
            while (idx < k_restricted) {
                Seg = combSeg[idx++];
                tmp_N_edges = 0;
                path_cost = 0;
                tmp = new LOSS_K[Constants.MAX_EL_ST];


                int[] res = Find_shortest_path_in_K(K, SP, Seg, tmp, tmp_N_edges);
                path_cost = res[0];
                tmp_N_edges = res[1];

                if (path_cost == IGNORE)
                    return IGNORE;

                if (path_cost < min) {
                    TmpBest_Loss_K = new LOSS_K[Constants.MAX_EL_ST];

                    System.arraycopy(tmp, 0, TmpBest_Loss_K, 0, tmp_N_edges);

                    TmpBest_N_edges = tmp_N_edges;
                    min = path_cost;
                }
            }

            System.arraycopy(TmpBest_Loss_K, 0, Loss_K, N_Forest_Loss_K, TmpBest_N_edges);
            N_Forest_Loss_K += TmpBest_N_edges;
        }

        N_Forest_Loss_K = RemoveDuplicatedEdges_In_Loss_K(N_Forest_Loss_K);

/* Count loss(K) */
        loss_k = 0;
        for (i = 0; i < N_Forest_Loss_K; i++) {
            loss_k += Loss_K[i].cost;
        }


        return loss_k;
    }

    int RemoveDuplicatedEdges_In_Loss_K(int org_N_edges) {
        int u, v, cost, i, j, no_edges, same;
        LOSS_K[] tmp = new LOSS_K[Constants.MAX_EL_ST];

        LOSS_K lossTMP = new LOSS_K();
        lossTMP.u = Loss_K[0].u;
        lossTMP.v = Loss_K[0].v;
        lossTMP.cost = Loss_K[0].cost;

        tmp[0] = lossTMP;

        no_edges = 1;
        for (i = 1; i < org_N_edges; i++) {
            u = Loss_K[i].u;
            v = Loss_K[i].v;
            cost = Loss_K[i].cost;
            same = 0;
            for (j = 0; j < no_edges; j++) {
                if ((u == tmp[j].u && v == tmp[j].v && cost == tmp[j].cost) ||
                        (u == tmp[j].v && v == tmp[j].u && cost == tmp[j].cost))
                    same = 1;
            }
            if (same == 0) {
                tmp[no_edges].u = u;
                tmp[no_edges].v = v;
                tmp[no_edges].cost = cost;
                no_edges++;
            }
        }
        Loss_K = new LOSS_K[Constants.MAX_EL_ST];
        System.arraycopy(tmp, 0, Loss_K, 0, no_edges);
        org_N_edges = no_edges;
        return org_N_edges;
    }


    int[] Find_shortest_path_in_K(SteinerT[] K, int SP, int Seg, LOSS_K[] tmp, int tmp_N_edges) {
        int i, peer, path_cost, edgeCost = -1, no_Loss_K, scannedEdge_in_K = -1, curr, curr_BP, degree;
        int[] BP;
        LOSS_K[] tmp_Loss_K = new LOSS_K[Constants.MAX_EL_ST];

        for (i = 0; i < N_edges_K; i++)
            K[i].touched = 0;

        curr_BP = -1;
        BP = new int[Constants.MAX_N_SP];
        no_Loss_K = 0;
        curr = SP;
        while (curr != Seg) {

            peer = -1;
            degree = 0;
            for (i = 0; i < N_edges_K; i++) {
                if (K[i].touched == 1) continue;
                if (K[i].u_id == curr) {
                    peer = K[i].v_id;
                    edgeCost = K[i].cellDist;
                    scannedEdge_in_K = i;
                    degree++;
                } else if (K[i].v_id == curr) {
                    peer = K[i].u_id;
                    edgeCost = K[i].cellDist;
                    scannedEdge_in_K = i;
                    degree++;
                }
            }

            if (degree > 1) {
                if (curr_BP == -1 ||
                        (curr_BP != -1 && curr != BP[curr_BP])) {
                    curr_BP++;
                    BP[curr_BP] = curr;
                }
            }

            if (peer == -1) {
                for (i = no_Loss_K - 1; i > -1; i--) {
                    if (tmp_Loss_K[i].u != curr && tmp_Loss_K[i].v != curr)
                        break;
                }
                no_Loss_K = i + 1;
                if (curr_BP == -1 ||
                        (curr_BP == 0 && curr == BP[curr_BP])) {
                    int[] res = new int[2];
                    res[0] = IGNORE;
                    res[1] = tmp_N_edges;
                    return res;
                }
                if (curr == BP[curr_BP]) curr_BP--;
                curr = BP[curr_BP];
                curr_BP--;
                continue;
            }

            if (IsSeg(peer) == 1 && peer != Seg) {
                K[scannedEdge_in_K].touched = 1;
                continue;
            }
            if (tmp_Loss_K[no_Loss_K] == null) {
                tmp_Loss_K[no_Loss_K] = new LOSS_K();
            }
            tmp_Loss_K[no_Loss_K].u = curr;
            tmp_Loss_K[no_Loss_K].v = peer;
            tmp_Loss_K[no_Loss_K].cost = edgeCost;
            no_Loss_K++;

            if (peer == Seg) {
                tmp_N_edges = no_Loss_K;

                System.arraycopy(tmp_Loss_K, 0, tmp, 0, tmp_N_edges);

                path_cost = 0;
                for (i = 0; i < tmp_N_edges; i++)
                    path_cost += tmp_Loss_K[i].cost;
                int[] res = new int[2];
                res[0] = path_cost;
                res[1] = tmp_N_edges;
                return res;
            }
            K[scannedEdge_in_K].touched = 1;
            curr = peer;
        }

        System.out.println("Logic error in Find_shortest_path_in_K\n");
        System.exit(0);
        return null;
    }

    /**
     * 0 if peer is a SP, namely N_seg <= peer < N_nodes_ H
     * 1 if peer is a segment, namely 0 <= peer < N_seg
     *
     * @param peer peer
     * @return xxx
     */
    int IsSeg(int peer) {
        if (0 <= peer && peer < N_seg) return 1;
        else return 0;
    }

    /* H = H U K */
    void Update_H() {
        int i;
        int N_edges_out;
        SteinerT[] out = new SteinerT[Constants.MAX_EL_ST];

        N_edges_out = 0;
        N_edges_out = Union_H_X_graphs(H, N_edges_H, best_K, best_N_edges_K, out);

        H = new SteinerT[Constants.MAX_EL_ST];
        System.arraycopy(out, 0, H, 0, N_edges_out);
        N_edges_H = N_edges_out;

        N_nodes_H = 0;
        node = new NODE[Constants.MAX_EL_ST];
        N_nodes_H = Count_nodes_of_graph(H, N_edges_H, node);
    }

    /* out = in1 union in2
So, it is possible that a pair of two end-points has more than one edges */

    int Union_H_X_graphs(SteinerT[] H, int N_edges_H, SteinerT[] in2, int N_edges_in2, SteinerT[] out) {
        int u_r, u_c, v_r, v_c, u_, u_r_, u_c_, v_, v_r_, v_c_,
                dist, dist_, same,
                i, j, N_edges_out, N_Seg_edges;

        N_Seg_edges = (N_seg * (N_seg - 1)) / 2;

        out = new SteinerT[Constants.MAX_EL_ST];

        System.arraycopy(H, 0, out, 0, N_edges_H);

        N_edges_out = N_edges_H;

        for (i = 0; i < N_edges_in2; i++) {

            u_r = in2[i].u_r;
            u_c = in2[i].u_c;
            v_r = in2[i].v_r;
            v_c = in2[i].v_c;
            dist = in2[i].cellDist;
            same = 0;
            for (j = N_Seg_edges; j < N_edges_H; j++) {
                u_r_ = H[j].u_r;
                u_c_ = H[j].u_c;
                v_r_ = H[j].v_r;
                v_c_ = H[j].v_c;
                dist_ = H[j].cellDist;
                if ((u_r == u_r_ && u_c == u_c_ && v_r == v_r_ && v_c == v_c_) ||
                        (u_r == v_r_ && u_c == v_c_ && v_r == u_r_ && v_c == u_c_)) {
                    same = 1;
                    if (dist < dist_) H[j].cellDist = dist;
                }
            }
            if (same == 0) {
                out[N_edges_out] = new SteinerT();
                out[N_edges_out].u_id = in2[i].u_id;
                out[N_edges_out].u_r = in2[i].u_r;
                out[N_edges_out].u_c = in2[i].u_c;
                out[N_edges_out].v_id = in2[i].v_id;
                out[N_edges_out].v_r = in2[i].v_r;
                out[N_edges_out].v_c = in2[i].v_c;
                out[N_edges_out].cellDist = dist;
                N_edges_out++;
            }

        }
        return N_edges_out;
    }

    /* T = MST( T U C[K] ) */
    void Update_T() {
        int N_nodes_T_U_CK, N_edges_MST_T_U_CK, N_edges_CK, N_edges_T_U_CK;
        SteinerT[] T_U_CK;
        SteinerT[] MST_T_U_CK;
        SteinerT[] CK;

//        N_edges_CK = 0;
        CK = new SteinerT[Constants.MAX_EL_ST];

        N_edges_CK = Create_CK(CK);


        T_U_CK = new SteinerT[Constants.MAX_EL_ST];
        N_edges_T_U_CK = Union_2_graphs(T, N_edges_T, CK, N_edges_CK, T_U_CK);

//        N_nodes_T_U_CK = 0;
        N_nodes_T_U_CK = Count_nodes_of_graph(T_U_CK, N_edges_T_U_CK, node);

//        N_edges_MST_T_U_CK = 0;
        MST_T_U_CK = new SteinerT[Constants.MAX_EL_ST];
        int[] res = Get_MST(T_U_CK, N_nodes_T_U_CK, N_edges_T_U_CK, MST_T_U_CK);
        cost_T = res[0];
        N_edges_MST_T_U_CK = res[1];
        T = new SteinerT[Constants.MAX_EL_ST];
        System.arraycopy(MST_T_U_CK, 0, T, 0, N_edges_MST_T_U_CK);

    }


    int Create_CK(SteinerT[] CK) {
        int u, v, cost, j, i, peer, N_edges_CK,
                Seg, SP = -1, the_other_seg;


        for (i = 0; i < best_N_edges_K; i++) {
            u = best_K[i].u_id;
            v = best_K[i].v_id;
            for (j = 0; j < best_N_Forest_Loss_K; j++) {
                if ((u == best_Loss_K[j].u && v == best_Loss_K[j].v) ||
                        (u == best_Loss_K[j].v && v == best_Loss_K[j].u)) {
                    best_K[i].touched = 1;
                    break;
                }
            }
            if (j == best_N_Forest_Loss_K)
                best_K[i].touched = 0;
        }

        N_edges_CK = 0;
        for (i = 0; i < best_N_edges_K; i++) { /* for each SP */

            if (best_K[i].touched == 1) continue;

            u = best_K[i].u_id;
            v = best_K[i].v_id;
            cost = best_K[i].cellDist;
            Seg = -1;
            if (IsSeg(u) == 1) {
                Seg = u;
                SP = v;
            } else if (IsSeg(v) == 1) {
                Seg = v;
                SP = u;
            }

            if (Seg == -1) {

                for (j = 0; j < best_N_Forest_Loss_K; j++)
                    best_Loss_K[j].touched = 0;
                Seg = Get_the_other_seg(u, u);
                SP = v;
            }

            for (j = 0; j < best_N_Forest_Loss_K; j++)
                best_Loss_K[j].touched = 0;
            the_other_seg = Get_the_other_seg(SP, SP);

            CK[N_edges_CK] = new SteinerT();
            CK[N_edges_CK].u_id = Seg;
            CK[N_edges_CK].u_r = segm.get(Seg).loc_r;
            CK[N_edges_CK].u_c = segm.get(Seg).loc_c;
            CK[N_edges_CK].v_id = the_other_seg;
            CK[N_edges_CK].v_r = segm.get(the_other_seg).loc_r;
            CK[N_edges_CK].v_c = segm.get(the_other_seg).loc_c;

            CK[N_edges_CK].cellDist = cost; /* important */
            N_edges_CK++;
            best_K[i].touched = 1;
        }
        return N_edges_CK;
    }

    int Get_the_other_seg(int Orgcurr, int curr) {
        int j, peer, scannedEdge = -1;

        peer = -1;
        for (j = 0; j < best_N_Forest_Loss_K; j++) {
            if (best_Loss_K[j].touched == 1) continue;
            if (best_Loss_K[j].u == curr) {
                scannedEdge = j;
                peer = best_Loss_K[j].v;
                break;
            } else if (best_Loss_K[j].v == curr) {
                scannedEdge = j;
                peer = best_Loss_K[j].u;
                break;
            }
        }
        if (peer == -1) {
            Get_the_other_seg(Orgcurr, Orgcurr);
            if (get_bug == 10) {
                System.out.println("get_bug error");
//                printf("Orgcurr:%d, curr:%d, peer:%d\n", Orgcurr, curr, peer);
//                print_Loss_K(best_Loss_K, best_N_Forest_Loss_K);
                System.exit(0);
            } else {
                get_bug++;
            }
        } else {

            if (IsSeg(peer) == 1) return peer;

            best_Loss_K[scannedEdge].touched = 1;
            Get_the_other_seg(Orgcurr, peer);
        }
        //todo check
        return -1;
    }

    int Count_RN_on_out(SteinerT[] st, int N_edges) {
        int i, n_rn = 0;
        double len;
        for (i = 0; i < N_edges; i++) {
            len = AnalyticGeometry.euclideanDistance(st[i].u_r, st[i].u_c, st[i].v_r, st[i].v_c);
            n_rn += Count_RNs_on_dist(len);
        }
        /* SP should be counted since any SP is an intermidate node */
        return n_rn;
    }

    int Count_RNs_on_dist(double len) {
        double R = Math.sqrt(2);
        int n_rn;
        if (len <= R)
            return 0;

        if (R < len && len <= 2 * R)
            return 1;
        else if (2 * R < len) {
            len -= (2 * R);
            n_rn = 2;

            while (len > R) {
                len -= R;
                n_rn++;
            }
            return n_rn;
        }
        //todo check
        return -1;
    }

    int Count_Total_Node_Dgr_MST_RNP(int N_RN) {
        return (N_RN + 1) * 2;
    }

    void Remove_duplicatedNodeID(SteinerT[] G, int N_edges) {
        int start, i, j, idx, r, c;

        start = (N_seg * (N_seg - 1)) / 2;
        for (i = start + 1; i < N_edges; i++) {
            idx = i;
/* For u_id */
            r = G[i].u_r;
            c = G[i].u_c;
            for (j = start; j < i; j++) {
                if (r == G[j].u_r && c == G[j].u_c)
                    G[idx].u_id = G[j].u_id;
                else if (r == G[j].v_r && c == G[j].v_c)
                    G[idx].u_id = G[j].v_id;
            }
            /* For v_id */
            r = G[i].v_r;
            c = G[i].v_c;
            for (j = start; j < i; j++) {
                if (r == G[j].u_r && c == G[j].u_c)
                    G[idx].v_id = G[j].u_id;
                else if (r == G[j].v_r && c == G[j].v_c)
                    G[idx].v_id = G[j].v_id;
            }
        }
    }


    void print_SteinerT(SteinerT[] in, int no_in_edges) {
        for (int i = 0; i < no_in_edges; i++) {
            System.out.print("{" + in[i].u_id + "(" + in[i].u_r + "," + in[i].u_c + ")-"
                    + in[i].v_id + "(" + in[i].v_r + "," + in[i].v_c + ")," + in[i].cellDist + "," + in[i].touched + "}, ");
        }
        System.out.println();
    }

    void print_node(int N_nodes) {
        System.out.print("node (id,r,c,included): ");
        for (int i = 0; i < N_nodes; i++) {
            System.out.print("(" + node[i].id + "," + node[i].r + "," + node[i].c + "," + node[i].included + "), ");
        }
        System.out.println("");
    }
}
